#include "gPlannerCollisionWrapper.h"
#include "omplapp/geometry/GeometrySpecification.h"
#include "PQP/PQP.h"

using namespace std;

namespace gplanner_collision_wrapper
{
	vector<std::vector<float> > getTriangleListFromScene (const std::vector<const aiScene*> &scenes, const std::vector<aiVector3D> &center)
	{
		vector<vector<float> > triangleList;

		vector<float> triangle(9);

		///

		for (unsigned int i = 0; i < scenes.size (); ++i)
		{
			if (scenes[i])
			{
				std::vector<aiVector3D> t;
				// extractTriangles is a misleading name.  this extracts the set of points,
				// where each set of three contiguous points consists of a triangle
				ompl::app::scene::extractTriangles (scenes[i], t);

				if (center.size () > i)
					for (unsigned int j = 0; j < t.size (); ++j)
						t[j] -= center[i];

				assert (t.size () % 3 == 0);

				for (unsigned int j = 0; j < t.size (); j+=3)
				{
					triangle[0] = t[j][0];
					triangle[1] = t[j][1];
					triangle[2] = t[j][2];

					triangle[3] = t[j+1][0];
					triangle[4] = t[j+1][1];
					triangle[5] = t[j+1][2];

					triangle[6] = t[j+2][0];
					triangle[7] = t[j+2][1];
					triangle[8] = t[j+2][2];

					triangleList.push_back(triangle);
				}
			}
		}

		///

		return triangleList;
	}

	PQP_Model* createPQPModel(const vector<vector<float> >& triangles)
	{
		PQP_Model* pTree = new PQP_Model;
		pTree->BeginModel(triangles.size());

		for(unsigned int i = 0; i < triangles.size(); ++i)
		{
			pTree->AddTri(&(triangles[i][0]), &(triangles[i][3]), &(triangles[i][6]), i);
		}
		pTree->EndModel(false);

		return pTree;
	}

	void setup(const ompl::app::GeometrySpecification* geometrySpecification, GMesh** obstacles, GMesh** robot)
	{
		// convert
		vector<vector<float> > obstacleTriangles = getTriangleListFromScene(geometrySpecification->obstacles, geometrySpecification->obstaclesShift);
		PQP_Model* obstacleModel = createPQPModel(obstacleTriangles);
		(*obstacles) = new GMesh(obstacleModel, obstacleTriangles);
		delete obstacleModel;

		vector<vector<float> > robotTriangles = getTriangleListFromScene(geometrySpecification->robot, geometrySpecification->robotShift);
		PQP_Model* robotModel = createPQPModel(robotTriangles);
		(*robot) = new GMesh(robotModel, robotTriangles);
		delete robotModel;
	}
};